#include "BambooDrive.h"
#include "WPILib.h"
#include "Math.h"

const int Trigger = 1;
const int Mecanum = 2;
const int Arcade = 3;
const int GyroButton = 6;
const float FR_CONST = 1;
const float FL_CONST = 1;
const float BR_CONST = 1;
const float BL_CONST = 1;
const float PI = 3.141592653589793238;

/*
 * Driving functions
 * These functions provide an interface to multiple motors that is used for C programming
 * The Drive(speed, direction) function is the main part of the set that makes it easy
 * to set speeds and direction independently in one call.
 */

/** Constructor for PandaDrive with 2 motors specified with channel numbers. 
 * Set up parameters for a two wheel drive system where the
 * left and right motor pwm channels are specified in the call.
 * This call assumes Victors for controlling the motors.
 * @param leftMotorChannel The PWM channel number on the default digital module that drives the left motor.
 * @param rightMotorChannel The PWM channel number on the default digital module that drives the right motor.
 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value).
 */
BambooDrive::BambooDrive(UINT32 frontRightMotorChannel, UINT32 frontLeftMotorChannel, 
		UINT32 backRightMotorChannel, UINT32 backLeftMotorChannel, DriverStationLCD *dslcd)
{
	m_frontRightMotor = new Jaguar(frontRightMotorChannel);
	m_frontLeftMotor = new Jaguar(frontLeftMotorChannel);
	m_backLeftMotor = new Jaguar(backLeftMotorChannel);
	m_backRightMotor = new Jaguar(backRightMotorChannel);
	m_dslcd = dslcd;
	
	m_counter = 0;
	m_gyro = false;
	m_gyroDiff = true;
	//bool m_Mecanum = true;
	//bool m_Gyro = true;
}

/**
 * Constructor for PandaDrive with 4 motors specified with channel numbers.
 * Set up parameters for a four wheel drive system where all four motor
 * pwm channels are specified in the call.
 * This call assumes Victors for controlling the motors.
 * @param frontLeftMotor Front left motor channel number on the default digital module
 * @param rearLeftMotor Rear Left motor channel number on the default digital module
 * @param frontRightMotor Front right motor channel number on the default digital module
 * @param rearRightMotor Rear Right motor channel number on the default digital module
 * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
 */

/**
 * Provide tank steering using the stored robot configuration.
 * Drive the robot using two joystick inputs. The Y-axis will be selected from
 * each Joystick object.
 * @param leftStick The joystick to control the left side of the robot.
 * @param rightStick The joystick to control the right side fo the robot.
 */

BambooDrive::~BambooDrive()
{
		delete m_frontLeftMotor;
		delete m_backLeftMotor;
		delete m_frontRightMotor;
		delete m_backRightMotor;
}

void BambooDrive::MecanumDrive(float xValue, float yValue, float zValue)
{

	/*if (Gyro) {
		XValue = sqrt(XValue*XValue+YValue*YValue)*atan2()
	}*/
	// square the inputs (while preserving the sign) to increase fine control while permitting full power
	xValue = Limit(xValue);
	yValue = -Limit(yValue);
	zValue = Limit(zValue);
	
	xValue *= fabs(xValue);
	yValue *= fabs(yValue);
	zValue *= fabs(zValue);

	float setFrontRight = yValue + xValue - zValue;
	float setFrontLeft = yValue - xValue + zValue;
	float setBackLeft = yValue + xValue + zValue;
	float setBackRight = yValue - xValue - zValue;
	
	SetAllWheelMotorSpeeds(setFrontRight, setFrontLeft, setBackRight, setBackLeft);
}

/**
 * Arcade drive implements single stick driving.
 * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
 * for the rotate value.
 * (Should add more information here regarding the way that arcade drive works.)
 * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
 * for forwards/backwards and the X-axis will be selected for rotation rate.  Pulling the trigger
 * enables "spin" functionality.
 */

void BambooDrive::MecanumDrive(GenericHID *stick) {
	MecanumDrive(stick->GetX(), stick->GetY(), stick->GetTwist());
}

void BambooDrive::ArcadeDrive(float xValue, float yValue) {
	
	yValue = -Limit(yValue);
	xValue = Limit(xValue);
	
	xValue *= fabs(xValue);
	yValue *= fabs(yValue);
	
	float setLeft = yValue + xValue;
	float setRight = yValue - xValue;

	SetTwoWheelMotorSpeeds(setLeft, setRight);
}

void BambooDrive::ArcadeDrive(GenericHID *stick) {
	ArcadeDrive(stick->GetX(), stick->GetY());
}

float BambooDrive::Limit(float num) {
	if (num > 1.0)
		return 1.0;
	if (num < -1.0)
		return -1.0;
	return num;
}

float BambooDrive::Max(float num1, float num2) {
	num1 = fabs(num1);
	num2 = fabs(num2);
	
	if (num1 <= 1 && num2 <= 1)
		return 1;
	
	if (num1 >= num2)
		return num1;
	else
		return num2;
}

float BambooDrive::Max(float num1, float num2, float num3, float num4) {
	num1 = fabs(num1);
	num2 = fabs(num2);
	num3 = fabs(num3);
	num4 = fabs(num4);
	
	if (num1 <= 1 && num2 <= 1 && num3 <= 1 && num4 <= 1)
		return 1;
	
	if (num1 >= num2) 
		if (num1 >= num3)
			if (num1 >= num4)
				return num1;
			else
				return num4;
		else if (num3 >= num4) 
				return num3;
			else
				return num4;
	else if (num2 >= num3)
			if (num2 >= num4)
				return num2;
			else
				return num4;
		else if (num3 >= num4)
				return num3;
			else
				return num4;
}

int BambooDrive::Teleop(GenericHID *leftStick, float gyroVal)
{
	float xVal;
	float yVal;
	
	static bool m_mecanum = true;
	//static bool m_Gyro = true;
	
	if (leftStick->GetRawButton(Mecanum))
			m_mecanum = true;
	if (leftStick->GetRawButton(Arcade))
			m_mecanum = false;
	
	
	if (leftStick->GetRawButton(GyroButton)) {
		if (m_gyroDiff) {
			if (m_gyro)
				m_gyro = false;
			else
				m_gyro = true;
		m_gyroDiff = false;
		}
	} else 
		m_gyroDiff = true;
		
	
	float joyX = leftStick->GetX();
	float joyY = leftStick->GetY();
	float twistVal = leftStick->GetTwist();
	
	if (m_gyro) {
		xVal = sqrt(joyX*joyX + joyY*joyY)*cos(-atan2(-joyY, joyX)-gyroVal*PI/180);
		yVal = sqrt(joyX*joyX + joyY*joyY)*sin(-atan2(-joyY, joyX)-gyroVal*PI/180);
	} else {
		xVal = joyX;
		yVal = joyY;
	}
	
	if (leftStick->GetTrigger())
		if (m_mecanum) {
			MecanumDrive(xVal, yVal, twistVal);
		} else {
			ArcadeDrive(joyX, joyY);
		}
	if (m_mecanum)
		if (m_gyro)
			return 3;
		else
			return 2;
	else
		return 1;
}

void BambooDrive::SetAllWheelMotorSpeeds(float frontRightSpeed, float frontLeftSpeed, float backRightSpeed, float backLeftSpeed) {
	float maxVal = Max(frontRightSpeed, frontLeftSpeed, backRightSpeed, backLeftSpeed);
	
	float FR = -FR_CONST*frontRightSpeed/maxVal;
	float FL = FL_CONST*frontLeftSpeed/maxVal;
	float BR = -BR_CONST*backRightSpeed/maxVal;
	float BL = BL_CONST*backLeftSpeed/maxVal;
	
	m_frontRightMotor->Set(FR);
	m_frontLeftMotor->Set(FL);
	m_backLeftMotor->Set(BL);
	m_backRightMotor->Set(BR);
	
	m_dslcd->Printf(DriverStationLCD::kUser_Line5, 1, "%.2f %.2f   ", FL, -FR);
	m_dslcd->Printf(DriverStationLCD::kUser_Line6, 1, "%.2f %.2f   ", BL, -BR);
	if (m_counter % 10000 == 0) 
		printf("\nMotor Speeds FR : %f FL : %f BL : %f BR : %f", frontRightSpeed, frontLeftSpeed, backLeftSpeed, backRightSpeed);
	m_counter++;
}

void BambooDrive::SetTwoWheelMotorSpeeds(float leftSpeed, float rightSpeed) {
	float maxVal = Max(leftSpeed, rightSpeed);
	
	float FR = -FR_CONST*rightSpeed/maxVal;
	float FL = FL_CONST*leftSpeed/maxVal;
	float BL = BL_CONST*leftSpeed/maxVal;
	float BR = -BR_CONST*rightSpeed/maxVal;
	
	m_frontRightMotor->Set(FR);
	m_frontLeftMotor->Set(FL);
	m_backLeftMotor->Set(BL);
	m_backRightMotor->Set(BR);
	
	m_dslcd->Printf(DriverStationLCD::kUser_Line5, 1, "%.2f %.2f   ", FL, -FR);
	m_dslcd->Printf(DriverStationLCD::kUser_Line6, 1, "%.2f %.2f   ", BL, -BR);
	if (m_counter % 10000 == 0)
		printf("\nMotor Speeds Right Speed: %f Left Speed: %f", rightSpeed, leftSpeed);
	m_counter++;
}
